#include "main.h"
#include "cmsis_os.h"
#include "motor.h"
#include "usart.h"
#include "ble_remote.h"

extern remote_t remote;
extern Motor motor1;
extern Motor motor2;
extern Motor motor3;
extern Motor motor4;

uint8_t button_pressed = 0; // 按钮状态标志

typedef enum
{
    MODE_RoboARM = 0, // 魔法阵模式
    MODE_REMOTE,      // 远程模式
    MODE_HILL,        // 上坡模式
    MODE_ESTOP,       // 急停模式
    MODE_DEFAULT      // 默认模式
} ControlMode;

#define BASE_SPEED 170    // 基础速度值
#define TURBO_SPEED 200  // 加速速度值
#define TURN_SPEED 150    // 转向速度值
#define STOP_SPEED 0      // 停止速度值
#define HILL_SPEED 260    // 上坡速度值
#define DEAD_ZONE 100     // 摇杆死区，防止误触发
#define SPEED_ADJUST 0.8f // 速度调整系数
#define PI 3.1415926535898f

// 函数声明
static void handle_remote_controls(const remote_t *r);
static void set_motors_speed(int32_t m1, int32_t m2, int32_t m3, int32_t m4);
static void stop_all_motors(void);
static void handle_joystick_control(const remote_t *r, ControlMode mode);
static int32_t adjust_speed_for_hill(int32_t base_speed);
static ControlMode get_current_mode(const remote_t *r);

// 图形绘制（测试用）
uint8_t send_data[2];

void Draw_ResponseCurve(UART_HandleTypeDef *huart, void *data, uint32_t size)
{
    uint8_t cmdf[2] = {3, ~3};
    uint8_t cmdr[2] = {~3, 3};
    HAL_UART_Transmit(huart, (uint8_t *)cmdf, sizeof(cmdf), 100);
    HAL_UART_Transmit(huart, (uint8_t *)data, size, 100);
    HAL_UART_Transmit(huart, (uint8_t *)cmdr, sizeof(cmdr), 100);
}

// 底盘控制任务
void StartDI_PANTask(void const *argument)
{

    // 初始化电机(到时候得改下引脚)
    Motor_Init(&motor1, &ENCODER_TIM1, &PWM_TIM4, PWM_TIM4_CH1, GPIOC, GPIO_PIN_12, GPIOD, GPIO_PIN_2);
    Motor_Init(&motor2, &ENCODER_TIM2, &PWM_TIM4, PWM_TIM4_CH2, GPIOA, GPIO_PIN_10, GPIOA, GPIO_PIN_11);
    Motor_Init(&motor3, &ENCODER_TIM3, &PWM_TIM4, PWM_TIM4_CH3, GPIOC, GPIO_PIN_3, GPIOC, GPIO_PIN_2);
    Motor_Init(&motor4, &ENCODER_TIM4, &PWM_TIM4, PWM_TIM4_CH4, GPIOA, GPIO_PIN_5, GPIOC, GPIO_PIN_4);
    // 初始化PID
    stop_all_motors();
    motor1.PidInit(&motor1, DELTA, 3600 - 1, 1800 - 1, 40, 3, 0);
    motor2.PidInit(&motor2, DELTA, 3600 - 1, 1800 - 1, 40, 3, 0);
    motor3.PidInit(&motor3, DELTA, 3600 - 1, 1800 - 1, 40, 3, 0);
    motor4.PidInit(&motor4, DELTA, 3600 - 1, 1800 - 1, 40, 3, 0);
    // 初始：停止所有电机
    stop_all_motors();
    while (1)
    {
        const remote_t *r = get_remote_control_point();

        if (r != NULL)
        {
            // 处理遥控指令（模式切换、按钮/摇杆控制）
            r = get_remote_control_point();
            handle_remote_controls(r);
            //  驱动电机
            Motor_Driver(&motor1, motor1.pid.out);
            Motor_Driver(&motor2, motor2.pid.out);
            Motor_Driver(&motor3, motor3.pid.out);
            Motor_Driver(&motor4, motor4.pid.out);
            /* send_data[0] = motor3.speed_set;
            send_data[1] = motor3.speed;
            Draw_ResponseCurve(&huart2, send_data, sizeof(send_data)); */
        }
        osDelay(20);
    }
}

// 处理遥控器指令
static void handle_remote_controls(const remote_t *r)
{
    button_pressed = 0; // 重置按钮状态
    ControlMode mode = get_current_mode(r);

    switch (mode)
    {
    // 魔法阵模式（按钮控制基本运动，摇杆用于精细调整）【用switch1启动】
    case MODE_RoboARM:
        stop_all_motors();
        osDelay(20);
        break;
    // 远程模式：使用摇杆进行精细控制
    case MODE_REMOTE:
        if (r->Button[0] == 1 | r->Button[1] == 1 | r->Button[2] == 1 | r->Button[3] == 1)
            button_pressed = 1;
        else
            button_pressed = 0;
        handle_joystick_control(r, MODE_REMOTE);
        break;
        //(通过handle_joystick_control（见下）来处理摇杆数据)

    // 上坡模式(增强扭矩输出)
    case MODE_HILL:
        if (r->Button[0] == 1 | r->Button[1] == 1 | r->Button[2] == 1 | r->Button[3] == 1)
            button_pressed = 1;
        else
            button_pressed = 0;
        handle_joystick_control(r, MODE_HILL);
        break;
    // 急停模式(立即停止所有电机)
    case MODE_ESTOP:
        stop_all_motors();
        break;
    }
}

// 设置所有电机速度
static void set_motors_speed(int32_t m1, int32_t m2, int32_t m3, int32_t m4)
{
    motor1.speed_set = m1;
    motor2.speed_set = m2;
    motor3.speed_set = m3;
    motor4.speed_set = m4;
    // 计算PID输出
    Motor_Calc(&motor1);
    Motor_Calc(&motor2);
    Motor_Calc(&motor3);
    Motor_Calc(&motor4);
}

// 停止所有电机
static void stop_all_motors(void)
{
    set_motors_speed(STOP_SPEED, STOP_SPEED, STOP_SPEED, STOP_SPEED);
}

// 处理摇杆控制
static void handle_joystick_control(const remote_t *remote, ControlMode mode)
{
    // 检查是否在死区内
    if (remote->rocker[0].distance < DEAD_ZONE && button_pressed == 0)
    {
        stop_all_motors();
        return;
    }
    // 1. 初始化速度变量
    int32_t vx = 0, vy = 0, wz = 0; // wz >0 -> 逆时针，wz <0 -> 顺时针
    int32_t base_speed = BASE_SPEED;
    int32_t hill_speed = HILL_SPEED;
    switch (mode)
    {
    case MODE_REMOTE: // 远程模式
        // 2.1 加速模式判断（switch1开启时用TURBO_SPEED）
        if (remote->Switch[1] == 1)
        {
            base_speed = TURBO_SPEED;
        }
        // 2.2 左摇杆：控制前后（vy）、左右（vx）
        if (remote->rocker[0].distance >= DEAD_ZONE)
        {
            vx = (int32_t)(remote->rocker[0].x_position * SPEED_ADJUST * base_speed / ROCKER_DISTANCE_MAX);
            vy = (int32_t)(remote->rocker[0].y_position * SPEED_ADJUST * base_speed / ROCKER_DISTANCE_MAX);
        }
				break;
    case MODE_HILL: // 上坡模式
        // 3
        if (remote->rocker[0].distance >= DEAD_ZONE)
        {
            vx = 0; // 上坡时禁止侧移
            if (remote->rocker[0].y_position > 0)
            {
                vy = hill_speed;
            }
            else if (remote->rocker[0].y_position < 0)
            {
                vy = (int32_t)(remote->rocker[0].y_position * SPEED_ADJUST * base_speed / ROCKER_DISTANCE_MAX);
            }
            else
                vy = 0;
        }
        break;
    default:
        break;
    }
    // 4. 按键：控制旋转（wz）
    if (remote->Button[2] == 1 && remote->Button[3] == 0)
    {
        wz = (int32_t)(1 * SPEED_ADJUST * TURN_SPEED);
    }
    else if (remote->Button[3] == 1 && remote->Button[2] == 0)
    {
        wz = (int32_t)((-1) * SPEED_ADJUST * TURN_SPEED);
    }
    else
        wz = 0;
    // 5. 麦轮运动学解算（标准4麦轮公式，适配常见布局：前左/前右/后左/后右）
    int32_t m1 = vy + vx - wz; // 前左电机
    int32_t m2 = vy - vx + wz; // 前右电机
    int32_t m3 = vy - vx - wz; // 后左电机
    int32_t m4 = vy + vx + wz; // 后右电机
    // 6. 设置电机速度
    set_motors_speed(m1, m2, m3, m4);
}

// 上坡速度调整
static int32_t adjust_speed_for_hill(int32_t base_speed)
{
    return (int32_t)(base_speed * 1.2f); // 上坡时提升20%
}

// 获取当前控制模式
static ControlMode get_current_mode(const remote_t *r)
{
    if (r->Switch[0] == 1)
        return MODE_RoboARM; // switch1：机械臂模式 */
    // no no,we don't need magic mode, we need roboarm mode
    if (r->Switch[2] == 1)
        return MODE_HILL; // switch3：上坡模式
    if (r->Switch[3] == 1)
        return MODE_ESTOP; // switch4：急停
    return MODE_REMOTE;    // 默认模式：：远程模式
}